/*
 * Copyright (C) 2019  Star Charge
 * File Name: modbus.c
 * Project: 通用
 * Description: modbus解包组包库
 * File Created: 2019-07-03
 * Author: szs
 * HISTORY:
 *      2019-07-03 初始版 
 */
#include <stdio.h>
#include <string.h>
#include "modbus.h"
#include "crc.h"

#if 0
#define LOG_DBG printf 
#else
#define LOG_DBG(...)
#endif

static MODBUS_REGISTER_T registerList[MODBUS_REGISTER_NUM_MAX]; /* 全局寄存器数组 */
static MODBUS_PROTOCOL_DEF_T protocolDef;                       /* 协议类型和从机地址 */

/* ModbusInit() - 初始化，指定协议和通讯收发接口函数
 * 输入 ：protocol - 协议
 *        unitAddress - rtu中为从机地址； tcp中为从机站号
 * 输出 ：0 - 成功; -1 - 失败
 */
int8 ModbusInit(uint8 protocol, uint8 unitAddress)
{
    if (protocol != MODBUS_RTU && protocol != MODBUS_TCP)
    {
        return -1;
    }
    protocolDef.protocol    = protocol;
    protocolDef.unitAddress = unitAddress;
    memset(registerList, '\0', sizeof(registerList));

    return 0;
}

/* ModbusAddRegisterGroup() - 批量注册寄存器
 * 输入:  from        - 地址起始(0 based)
 *        to          - 结束地址
 *        rw          - 访问权限(全组统一，如单设需调用ModbusAddRegister())
 *        initValue   - 寄存器初始化值
 * 输出:  0 - 成功; -1 - 失败
 */
int8 ModbusAddRegisterGroup(uint16 from, uint16 to, uint8 rw, uint16 initValue)
{
    uint16 index;
    
    if (from > to || 
        from >= MODBUS_REGISTER_NUM_MAX ||
        to   >= MODBUS_REGISTER_NUM_MAX  )
    {
        return -1;
    }
    
    for (index = from; index <= to; index++)
    {
        registerList[index].rw      = (rw < MODBUS_READ_ONLY || rw > MODBUS_READ_WRITE) ? MODBUS_READ_WRITE : rw;
        registerList[index].value   = initValue;
    }
    
    return 0;
}

/* ModbusAddRegister() - 注册单个寄存器
 * 输入:address - 地址值(0 based)
 *      rw - 访问权限
 *      initValue - 寄存器初始化值
 * 输出:0 - 成功; -1 - 失败
 */
int8 ModbusAddRegister(uint16 address, uint8 rw, uint16 initValue)
{
    return ModbusAddRegisterGroup(address, address,  rw, initValue);
}

/* ModbusReadRegister() - 读取寄存器值
 * 输入： address - 地址值
 * 输出： > 0XFFFF - 失败；其他值 - 后16位为寄存器值  
 * 备注：返回值为uint32，当成功返回后，调用者需用uint16转换
 */
uint32 ModbusReadRegister(uint16 address)
{    
    if (address < MODBUS_REGISTER_NUM_MAX)
    {   
        if (registerList[address].rw)
        {
            return registerList[address].value;
        }
    }
    
    return 0X00010000;
}

/* ModbusWriteRegister() - 写寄存器值
 * 输入:  address  - 地址
 *        value    - 值
 * 输出：-1 失败；0 - 成功  
 */
int8 ModbusWriteRegister(uint16 address, uint16 value)
{   
    if (address < MODBUS_REGISTER_NUM_MAX)
    {      
        if (registerList[address].rw) 
        {
            registerList[address].value = value;
            return 0;
        }
    }
    
    return -1; 
}

#define MODBUS_BUILD_ERROR_RESPONSE(errorCode) do \
{\
    uint8 crcHi, crcLo;\
    buffer[dataOffset    ]  = protocolDef.unitAddress;\
    buffer[dataOffset + 1]  += 0X80;\
    buffer[dataOffset + 2]  = errorCode;\
    if (protocol == MODBUS_RTU)\
    {\
        crcHi = CRC16( buffer, 3, 1 );\
        crcLo = CRC16( buffer, 3, 0 );\
        buffer[dataOffset + 3] = crcHi;\
        buffer[dataOffset + 4] = crcLo;\
        *outLen = dataOffset + 5;\
    }\
    else\
    {\
      buffer[dataOffset - 2] = 0;\
      buffer[dataOffset - 1] = 3;\
      *outLen = dataOffset + 3;\
    }\
}while(0)
    
/* ModBusHandleRequest() - 处理接收到的Modbus数据包，根据当前的协议类型，
 *                    解包，读取/设置，组包，其中接收缓存与发送缓存共用
 * 输入：buffer - 数据包缓存起始地址
 *       
 * 输出：buffer - 同时作为输出缓存的起始地址, outLen - 返回的有效数据缓存长度
 * 返回：0 - 成功； errorCode - 错误码  
 *       
 * 备注：
 *     错误检查包括：
 *          1.寄存器地址合法性；2.数据包CRC校验；3.读写权限；
 *          4.数据包功能码合法性；5.从设备号正确性
 *
 *     errorCode列表：
 *          -1 --> buffer不可用 01 --> 非法功能码，02 --> 非法数据地址（从设备中不存在） 
 *          03 --> 非法数据值（超过范围或者不允许使用） 04-->从设备出现未知错误
 *       
 *     数据包格式样例:
 *          [读寄存器]      
 *          RTU:
 *          主机：[01] [03] [00 01] [00 01] [D5 CA]  
 *          0 [01]-从机地址
 *          1 [03]-功能码，读取寄存器
 *          2-3 [00 01]-寄存器地址
 *          4-5 [00 01]-需要读取的寄存器个数（16位）
 *          6-7 crc
 *          从机：[01] [03] [02] [0017]  [F8 4A]
 *          0 [01]-从机地址
 *          1 [03]- 功能码，读取寄存器
 *          2 [02]- 返回字节个数 
 *          3 - 4[00 17]-寄存器值
 *
 *          TCP:
 *          Client请求：
 *          [19 B2][00 00][00 06 ][01][03] [00 01][00 01] 
 *          [19 B2]-事务标识符
 *          [00 00]-代表TCP协议
 *          [00 06 ]-后面剩余字节数
 *          [01]-站号
 *          [03]-功能码
 *          [00 01]-寄存器地址
 *          [00 01] –需要读取的寄存器个数（16位）
 *          Serve回复：
 *          [19 B2][00 00][00 07][06][03][02][00 00]
 *          [03]-功能码
 *          [02]-返回的字节个数
 *          [00 00] –寄存器值 
 *
 *          [写单个（请求和响应格式一样）]
 *          0   Slave Address
 *          1   Function
 *          2   Register Address Hi
 *          3   Register Address Lo
 *          4   Preset Data Hi
 *          5   Preset Data Lo
 *          6   Error Check (Hi)
 *          7   Error Check (Lo)
 *
 *          [写多个请求]
 *          0  Slave Address
 *          1  Function
 *          2  Starting Address Hi
 *          3  Starting Address Lo
 *          4  No. of Registers Hi
 *          5  No. of Registers Lo
 *          6  Byte Count
 *          7  Data Hi
 *          8  Data Lo
 *          9  Data Hi
 *          10  Data Lo
 *          11-12  Error Check (LRC or CRC)
 *          [写多个回复]
 *          0  Slave Address
 *          1  Function
 *          2  Starting Address Hi
 *          3  Starting Address Lo
 *          4  No. of Registers Hi
 *          5  No. of Registers Lo
 *          6-7  Error Check (LRC or CRC)
 */
int8 ModBusHandleRequest(uint8 *buffer, uint16 *outLen)
{
    /* 因1.长度不定;2.中间仍需对字段位置定位后再操作；3.读写时的数据包格式不统一, 故不使用结构体直接映射buffer */
    
    /* 应用级变量 */
    uint8  protocol   = protocolDef.protocol;
    uint8  dataOffset = (protocol == MODBUS_RTU) ? 0 : 6;
    
    /*协议共用变量*/
    uint8  funcNo;
    uint8  unitAddress;
    uint16 registerStartAddress;
    
    /* 读和多写操作变量*/
    uint8  byteCount;
    uint8  returnBytes = 0;
    uint16 registerNum;
    uint16 writeValue;
    
    /* RTU变量 */
    uint8  crcHi;
    uint8  crcLo;

    /* 循环逻辑变量 */
    uint16 index;

    
    if (NULL == buffer || NULL == outLen)
    {
        return MODBUS_INVALID_BUFFER;
    }
    
    *outLen = 0;
    PrintBuffer(buffer, 30);

    unitAddress          = buffer[dataOffset];
    funcNo               = buffer[dataOffset + 1];
    registerStartAddress = (buffer[dataOffset + 2] << 8) + buffer[dataOffset + 3];
    
    /* 单写操作没有此字段 */
    registerNum          = (buffer[dataOffset + 4] << 8) + buffer[dataOffset + 5]; 

    LOG_DBG("local protocol:%u unitAddress:%u\r\n", protocolDef.protocol, protocolDef.unitAddress);
    LOG_DBG("dataoff:%u unitAddress:%u funcNo:%u registerStartAddress:%u, registerNum:%u\r\n", 
            dataOffset, unitAddress, funcNo, registerStartAddress, registerNum);

    /* 检查从机地址 */
    if (unitAddress != protocolDef.unitAddress)
    {
        /* BuildResponse() */
    //    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_REGISTER);/* delete by Sophy 杨晓菲, 20190718 */
        return MODBUS_INVALID_BUFFER;		/* modify by Sophy 杨晓菲, 20190718 */
    }
    
    /* 检查功能码合法性 */
    if (funcNo < MODBUS_FUNCNO_MIN || funcNo > MODBUS_FUNCNO_MAX)
    {
        /* BuildResponse() */
        MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
        return MODBUS_INVALID_FUNC;      
    }
    
    /* 检查寄存器地址是否越界 */
    if (registerStartAddress >= MODBUS_REGISTER_NUM_MAX)
    {
        MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_REGISTER);
        return MODBUS_INVALID_REGISTER;    
    }
    else if (funcNo == MODBUS_WRITE_MULTI || funcNo == MODBUS_READ)
    {
        /* 检查寄存器请求数量是否合法 */
        if (registerNum > MODBUS_REGISTER_REQUEST_NUM_MAX)
        {
            MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_REGISTER);
            return MODBUS_INVALID_REGISTER;   
        }
        
        /* 检查寄存器地址是否已注册 */
        for (index = registerStartAddress; index < registerStartAddress + registerNum; index++)
        {
            /* rw字段可以作为用来判断该寄存器是否已经被注册的flag */
            if (registerList[index].rw == 0)
            {
                /* BuildResponse() */
                MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_REGISTER);
                return MODBUS_INVALID_REGISTER;  
            }

            if (funcNo == MODBUS_WRITE_MULTI)
            {
                if (registerList[index].rw == MODBUS_READ_ONLY)
                {
                     MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                     return MODBUS_INVALID_FUNC;
                }
            }
            else if (funcNo == MODBUS_READ)
            {
                if (registerList[index].rw == MODBUS_WRITE_ONLY)
                {
                     MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                     return MODBUS_INVALID_FUNC;
                }
            }
        }
    }

    switch (funcNo)
    {
        case MODBUS_READ:
            /* RTU需要CRC校验 */
            if (protocol == MODBUS_RTU)
            {
                crcHi = CRC16( buffer, 6, 1 );
                crcLo = CRC16( buffer, 6, 0 );

                if (crcHi != buffer[6] || crcLo != buffer[7])
                {
                    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                    return MODBUS_INVALID_FUNC;    
                }
            }
            
            for (index = 0; index < registerNum; index++)
            {
                uint32 readValue;

                readValue = ModbusReadRegister(index + registerStartAddress);
                if (readValue > 0XFFFF)
                {
                    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_UNKNOWN_ERROR);
                    return MODBUS_UNKNOWN_ERROR;  
                }
                buffer[dataOffset + 3 + index * 2] = (readValue >> 8) & 0XFF;
                buffer[dataOffset + 4 + index * 2] = (readValue)      & 0XFF;
                returnBytes += 2;
            }
            
            buffer[dataOffset + 2] = returnBytes;

            /* RTU需要加CRC校验 */
            if (protocol == MODBUS_RTU)
            {
                crcHi = CRC16( buffer, 3 + returnBytes, 1 );
                crcLo = CRC16( buffer, 3 + returnBytes, 0 );
                buffer[dataOffset + 3 + returnBytes]     = crcHi; 
                buffer[dataOffset + 3 + returnBytes + 1] = crcLo; 
                *outLen = dataOffset + 3 + returnBytes + 2;
            }
            else /* TCP需要修改头部中[后面剩余字节数]字段*/
            { 
                buffer[4] = 0;
                buffer[5] = (3 + returnBytes);
                *outLen = dataOffset + 3 + returnBytes;
            } 
            
        break;
        
        case MODBUS_WRITE_MULTI:
            byteCount = buffer[6];
            /* RTU需要CRC校验 */
            if (protocol == MODBUS_RTU)
            {
                crcHi = CRC16( buffer, 7 + byteCount, 1 );
                crcLo = CRC16( buffer, 7 + byteCount, 0 );

                if (crcHi != buffer[7 + byteCount] || crcLo != buffer[7 + byteCount + 1])
                {
                    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                    return MODBUS_INVALID_FUNC;    
                }
            }
            
            for (index = 0; index < registerNum; index++)
            {
                writeValue = (buffer[dataOffset + 7 + index * 2] << 8) +  buffer[dataOffset + 8 + index * 2];
 
                if (-1 == ModbusWriteRegister(index + registerStartAddress, writeValue))
                {
                    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_UNKNOWN_ERROR);
                    return MODBUS_UNKNOWN_ERROR; 
                }

                returnBytes += 2;
            }
            
            //buffer[dataOffset + 2] = returnBytes;
            
            *outLen = dataOffset + 6;

            /* RTU需要加CRC校验 */
            if (protocol == MODBUS_RTU)
            {
                crcHi = CRC16( buffer, 6, 1 );
                crcLo = CRC16( buffer, 6, 0 );
                buffer[dataOffset + 6] = crcHi; 
                buffer[dataOffset + 7] = crcLo; 
                *outLen += 2;
            }
            else /* TCP需要修改头部中[后面剩余字节数]字段*/
            { 
                buffer[4] = 0;
                buffer[5] = 6;
            }
            
        break;
        
        case MODBUS_WRITE_ONE:
            /* RTU需要CRC校验 */
            if (protocol == MODBUS_RTU)
            {
                crcHi = CRC16( buffer, 6, 1 );
                crcLo = CRC16( buffer, 6, 0 );

                if (crcHi != buffer[6] || crcLo != buffer[7])
                {
                    MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                    return MODBUS_INVALID_FUNC;    
                }

                /* RTU多两个字节放CRC */
                *outLen = 2;
            }

            /* 判断是否只读 */
            if (registerList[registerStartAddress].rw == MODBUS_READ_ONLY) 
            {
                MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
                return MODBUS_INVALID_FUNC;
            }
             /* 判断是否存在 */
            if (registerList[registerStartAddress].rw == 0)
            {
                MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_REGISTER);
                return MODBUS_INVALID_REGISTER;  
            }
            
            writeValue = (buffer[dataOffset + 4] << 8) +  buffer[dataOffset + 5];
            if (-1 == ModbusWriteRegister(registerStartAddress, writeValue))
            {
                MODBUS_BUILD_ERROR_RESPONSE(MODBUS_UNKNOWN_ERROR);
                return MODBUS_UNKNOWN_ERROR; 
            }
            
            *outLen += dataOffset + 6;
            
            /* 单写操作正常回复数据包和请求包一样，不用修改 */
        break; 
        default:
            MODBUS_BUILD_ERROR_RESPONSE(MODBUS_INVALID_FUNC);
            return MODBUS_INVALID_FUNC; 
    }
    
    return 0;
}


/* PrintBuffer() - 打印协议数据包缓存内容
 * 输入 ：none
 *        
 * 输出 ：none
 */
void PrintBuffer(uint8 *buffer, int32 len)
{
    int32 i;
    
    for (i = 0; i < len; i++)
    {
        LOG_DBG("%02X ", buffer[i]);
        if (i > 0 && i % 40 == 0)
        LOG_DBG("\r\n");
    }
    LOG_DBG("\r\n");
}

/* ModbusPrintRegisterList() -打印寄存器列表
 * 输入 ：none
 *        
 * 输出 ：none
 */
void ModbusPrintRegisterList()
{
    uint16 index;

    for (index = 0; index < MODBUS_REGISTER_NUM_MAX; index++)
    {
        LOG_DBG("[%u] rw:%u value :%04x ", index, registerList[index].rw, registerList[index].value);
        if (index > 0 && index % 5 == 0)
        {
            LOG_DBG("\r\n");
        }
    }
    LOG_DBG("\r\n");
}
